1. Field of the Invention
This invention relates to a velocity control apparatus for controlling the velocity of the distal end of a load, such as the arm of a robot, by a status observing apparatus.
2. Description of the Related Art
Servomotors are used in control systems for driving various mechanical loads in machine tools and the like. In the case of a servomotor for driving a load having little rigidity such as a robot arm, it is required that velocity be controlled and not just position.
FIG. 2 is a block diagram illustrating an example of a conventional servo-control system having a mechanical load with little rigidity. A servo-controller a is included in a control system constructed such that a servomotor b and a mechanical load c are connected by a spring constant (K) d.
In FIG. 2, Kt denotes a torque constant, Jm rotor inertia, K the spring constant, J.sub.L load inertia and l/s an integration term. In a case where the mechanical load c having little rigidity is driven by the servomotor b, a current command u(k) applied to the control system is converted into a torque command by the torque constant Kt and a deviation signal between this torque command and an actual torque value is applied to the servomotor b as a command. This deviation signal is converted into an acceleration command by the rotor inertia term Jm, and this command is passed through the integration term once to obtain a motor velocity command signal V for driving and controlling the servomotor b to indirectly control the mechanical load c.
The motor velocity command signal V is passed through the integration term to obtain a position command signal .theta., which is used to determine velocity V.sub.L and position .theta..sub.L for the mechanical load c. Connected to an output shaft of the servomotor b is a rotary encoder e or the like for detecting, as a command signal y, the commanded position of the mechanical load c commanded by the servomotor b.
The operating states of the servomotor b and mechanical load c constituting the actual control system as an actuator cannot be fed back to the servocontroller upon measuring all of the variables thereof on-line. Accordingly, the necessary status data regarding the object under control must be estimated upon stipulating the state of the control system based solely on utilizable data from the rotary encoder e or the like. It is therefore necessary to detect the motor velocity command signal V, the actual velocity V.sub.L at the distal end of the mechanical load and control velocity in dependence upon the deviation with respect to the command signal V. In general, a sensor for detecting the velocity V.sub.L of the mechanical load c is high in cost and is difficult to mount on every axis of a robot. For these reasons, velocity control in the servo-control system shown in FIG. 2 is performed by estimating velocity by means of an observer (status observing device).
In cases where status variables cannot be measured directly, the status variables are reproduced by the observer from a control input and a measurement output. However, in the foregoing servo-control system, the estimated value of velocity based on the servomotor b and the position of the distal end of the mechanical load c are computed and discrete value control is carried out by a microprocessor (now shown). The following computation equations serve as a basis for estimated values .theta., V of position and velocity of the motor and estimated values .theta..sub.L, V.sub.L of the position and velocity of the load used in this computation: EQU .theta.(K+1)=(1-K1){0.sub.11 .theta.(k)+0.sub.12 V(K)+0.sub.13 .theta..sub.L (K) +0.sub.14 V.sub.L (K)+h.sub.1 u(K)}+K.sub.1y (K+1)(1) EQU V(K+1)=(0.sub.21 -K.sub.2 0.sub.11).theta.(K)+(0.sub.22 31 K.sub.2 0.sub.12)V(K)+(0.sub.23 31 K.sub.2 0.sub.13).theta..sub.L (k) +(0.sub.24 -K.sub.2 0..sub.14)VL(K)+(h.sub.2 -K.sub.2 H.sub.1)U(K) +K.sub.2y (K+1)(2) EQU .theta.(K+1)=(0.sub.31 -K.sub.3 0.sub.11).theta.(K)+(0.sub.32 -K.sub.3 0.sub.12)V(K)+(0.sub.23 -K.sub.3 0.sub.13).theta.L(K) +(0.sub.34 -K.sub.3 0.sub.14)VL(K)+(h.sub.3 -K.sub.3 h.sub.1)U(K) +K.sub.3y (K+1)(3) EQU V.sub.L (K+1)=(0.sub.41 -K.sub.4 0.sub.11).theta.(K)+(0.sub.42 -K.sub.4 0.sub.12)V(K)+(0.sub.43 -K.sub.4 0.sub.13).theta.L(K) +(0.sub.44 -K.sub.4 0.sub.14)VL(K)+(h.sub.4 -k.sub.4 h.sub.1)U(K) +k.sub.4y (K+1)(4)
where K.sub.1 -K.sub.4 are the gains of the observer.
A differential equation [Eq. (7)] regarding a status matrix (k) of a discrete value system is expressed as follows: ##EQU1##
However, in order to obtain the estimated values of motor position and velocity as well as of the position and velocity of the distal end of the mechanical load with the observer in the servo-control system shown in FIG. 2, it is required that multiplication processing be performed a large number of times, as indicated by the Eqs. (1)-(4). In other words, processing regarding the coefficient matrix performed by the microprocessor requires a long period of time and the velocity of the distal end of a robot arm cannot be accurately controlled.